﻿/*------------------------------------------------------------------------

C3DMATHCPU.HPP v1.1.5

Copyright 楊志賢 CxxlMan, 2012 - 2014
All Rights Reserved

------------------------------------------------------------------------*/

#if !defined(__C3DMATHCPU_HPP_Cxxl3D)
#define __C3DMATHCPU_HPP_Cxxl3D


#include <math.h>
#include <Sys\VECTOR.HPP>
#include <Sys\CSMART_PTR.HPP>
#include <Sys\MATRIX.HPP>

#define MATH_PI 3.141592653f

namespace Cxxl3D
{

// 角度轉弧度
inline C3D_FLOAT cxxlFASTCALL DegreeToRadian(C3D_FLOAT r)
{
	float radian = r * (MATH_PI / 180.0f);
	return radian;
}


inline C3D_FLOAT cxxlFASTCALL Cot(C3D_FLOAT radian)
{
  C3D_FLOAT result;

  __asm
  {
    fld [radian]
    fptan
    fdivrp st(1), st(0)
    fstp [result]
  }

  return result;
}

inline void cxxlFASTCALL SinCos(C3D_FLOAT radian, C3D_FLOAT &sinvalue, C3D_FLOAT &cosvalue)
{
  __asm
  {
    mov eax, dword ptr[cosvalue]
    mov ebx, dword ptr[sinvalue]
    fld [radian]
    fsincos
    fstp dword ptr [eax]
    fstp dword ptr [ebx]
  }
}

inline C3D_FLOAT Sqrt(C3D_FLOAT v)
{
	float result;
	__asm
	{
		fld [v]
		fsqrt
		fstp [result]
	}
	return result;
}



/**********************************
** Vector4 函數
**********************************/

// 三維向量 * 值
inline C3DVector4 cxxlFASTCALL operator*(const C3DVector4 &v, const C3D_FLOAT Value)
{
  return C3DVector4(v.x*Value, v.y*Value, v.z*Value);
}

// 三維向量 * 值
inline Vector4 cxxlFASTCALL operator*(const Vector4 &v, const C3D_FLOAT Value)
{
//  CxxlMan::CriticalSecHandle AutoCSH(v.csObject);

  return Vector4(v.x*Value, v.y*Value, v.z*Value);
}



// 值 * 三維向量
inline C3DVector4 cxxlFASTCALL operator*(const C3D_FLOAT Value, const C3DVector4 &v)
{
  return C3DVector4(v.x*Value, v.y*Value, v.z*Value);
}

// 值 * 三維向量
inline Vector4 cxxlFASTCALL operator*(const C3D_FLOAT Value, const Vector4 &v)
{
//  CxxlMan::CriticalSecHandle AutoCSH(v.csObject);

  return Vector4(v.x*Value, v.y*Value, v.z*Value);
}


// 三維向量 + 三維向量
inline C3DVector4 cxxlFASTCALL operator+(const C3DVector4 &lhs, const C3DVector4 &rhs)
{
  return C3DVector4(lhs.x+rhs.x, lhs.y+rhs.y, lhs.z+rhs.z);
}

// 三維向量 + 三維向量
inline Vector4 cxxlFASTCALL operator+(const Vector4 &lhs, const Vector4 &rhs)
{
//  C3DVector4 lhs_tmp = lhs.Get_C3DVector4();

//  CxxlMan::CriticalSecHandle AutoCSH(rhs.csObject);

  return Vector4(lhs.x+rhs.x, lhs.y+rhs.y, lhs.z+rhs.z);
}



// 三維向量 - 三維向量
inline C3DVector4 cxxlFASTCALL operator-(const C3DVector4 &lhs, const C3DVector4 &rhs)
{
  return C3DVector4(lhs.x-rhs.x, lhs.y-rhs.y, lhs.z-rhs.z);
}

// 三維向量 - 三維向量
inline Vector4 cxxlFASTCALL operator-(const Vector4 &lhs, const Vector4 &rhs)
{
//  C3DVector4 lhs_tmp = lhs.Get_C3DVector4();

//  CxxlMan::CriticalSecHandle AutoCSH(rhs.csObject);

  return Vector4(lhs.x-rhs.x, lhs.y-rhs.y, lhs.z-rhs.z);
}


// - 三維向量
inline C3DVector4 cxxlFASTCALL operator-(const C3DVector4 &v)
{
  return C3DVector4(-v.x, -v.y, -v.z, v.w);
}

// - 三維向量
inline Vector4 cxxlFASTCALL operator-(const Vector4 &v)
{
  return Vector4(-v.x, -v.y, -v.z, v.w);
}


// 三維向量 / 值
inline C3DVector4 cxxlFASTCALL operator/(const C3DVector4 &v, const C3D_FLOAT Value)
{
  return C3DVector4(v.x/Value, v.y/Value, v.z/Value);
}

// 三維向量 / 值
inline Vector4 cxxlFASTCALL operator/(const Vector4 &v, const C3D_FLOAT Value)
{
  //CxxlMan::CriticalSecHandle AutoCSH(v.csObject);

  return Vector4(v.x/Value, v.y/Value, v.z/Value);
}



// 求出三維內積
inline C3D_FLOAT cxxlFASTCALL Vector3Dot(const C3DVector4 &a, const C3DVector4 &b)
{
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

// 求出三維內積
inline C3D_FLOAT cxxlFASTCALL Vector3Dot(const Vector4 &a, const Vector4 &b)
{
//  C3DVector4 a_tmp = a.Get_C3DVector4();

//  CxxlMan::CriticalSecHandle AutoCSH(b.csObject);

  return a.x * b.x + a.y * b.y + a.z * b.z;
}



// 求出三維外積
inline C3DVector4 cxxlFASTCALL Vector3CrossProduct(const C3DVector4 &a, const C3DVector4 &b)
{
/*
	C3DVector4 c;

	c.x = a.y * b.z - a.z * b.y;
	c.y = a.z * b.x - a.x * b.z;
	c.z = a.x * b.y - a.y * b.x;
	c.w = 0.0f;

	return c;
*/

  return C3DVector4(a.y * b.z - a.z * b.y,
                   a.z * b.x - a.x * b.z,
                   a.x * b.y - a.y * b.x,
                   0);
}

// 求出三維外積
inline Vector4 cxxlFASTCALL Vector3CrossProduct(const Vector4 &a, const Vector4 &b)
{
//  C3DVector4 a_tmp = a.Get_C3DVector4();

//  CxxlMan::CriticalSecHandle AutoCSH(b.csObject);

//	C3DVector4 c;

/*
	c.x = a_tmp.y * b.z - a_tmp.z * b.y;
	c.y = a_tmp.z * b.x - a_tmp.x * b.z;
	c.z = a_tmp.x * b.y - a_tmp.y * b.x;
	c.w = 0.0f;

	return c;
*/
  return Vector4(a.y * b.z - a.z * b.y,
                 a.z * b.x - a.x * b.z,
                 a.x * b.y - a.y * b.x,
                 0);
}



// 取得向量長度
inline C3D_FLOAT cxxlFASTCALL Vector3Length(const C3DVector4 &a)
{
  return Sqrt(a.x*a.x + a.y*a.y + a.z*a.z);
}

// 取得向量長度
inline C3D_FLOAT cxxlFASTCALL Vector3Length(const Vector4 &a)
{
//  CxxlMan::CriticalSecHandle AutoCSH(a.csObject);

  return Sqrt(a.x*a.x + a.y*a.y + a.z*a.z);
}




// 三維正規化
inline void cxxlFASTCALL Vector3Normalize(C3DVector4 &a)
{
  C3D_FLOAT len = Sqrt(a.x*a.x + a.y*a.y + a.z*a.z);

  a.x = a.x / len;
  a.y = a.y / len;
  a.z = a.z / len;
}

// 三維正規化
inline void cxxlFASTCALL Vector3Normalize(Vector4 &a)
{
//  CxxlMan::CriticalSecHandle AutoCSH(a.csObject);

  C3D_FLOAT len = Sqrt(a.x*a.x + a.y*a.y + a.z*a.z);

  a.x = a.x / len;
  a.y = a.y / len;
  a.z = a.z / len;
}

// 三維正規化
inline C3DVector4 cxxlFASTCALL Vector3Normalize(const C3D_FLOAT x, const C3D_FLOAT y, const C3D_FLOAT z)
{
  C3D_FLOAT len = Sqrt(x*x + y*y + z*z);


  return C3DVector4(x/len, y/len, z/len);
}


/*************************
** Matrix4x4 函數
*************************/

// 產生位移的矩陣
inline Matrix4x4 cxxlFASTCALL Trsnalte(C3D_FLOAT x, C3D_FLOAT y, C3D_FLOAT z)
{
  return Matrix4x4(
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    x, y, z, 1
  );
}

// 產生圍繞 X 軸旋轉的矩陣
inline Matrix4x4 cxxlFASTCALL RotateX(const C3D_FLOAT radian)
{
  float fSin, fCos;
	SinCos(radian, fSin, fCos);

  return Matrix4x4(
    1.0f,  0.0f,  0.0f, 0.0f,
    0.0f,  fCos,  fSin, 0.0f,
    0.0f,  -fSin,  fCos, 0.0f,
    0.0f,  0.0f,  0.0f, 1.0f
  );
}


// 產生圍繞 Y 軸旋轉的矩陣
inline Matrix4x4 cxxlFASTCALL RotateY(const C3D_FLOAT radian)
{
	float fSin, fCos;
	SinCos(radian, fSin, fCos);

  return Matrix4x4(
    fCos,  0.0f, -fSin,  0.0f,
    0.0f,  1.0f,  0.0f,  0.0f,
    fSin,  0.0f,  fCos,  0.0f,
    0.0f,  0.0f,  0.0f,  1.0f
  );
}

// 產生圍繞 Z 軸旋轉的矩陣
inline Matrix4x4 cxxlFASTCALL RotateZ(const C3D_FLOAT radian)
{
	float fSin, fCos;
	SinCos(radian, fSin, fCos);

  return Matrix4x4(
    fCos, fSin, 0.0f, 0.0f,
   -fSin, fCos, 0.0f, 0.0f,
    0.0f, 0.0f, 1.0f, 0.0f,
    0.0f, 0.0f, 0.0f, 1.0f
  );
}


// 產生圍繞指定向量旋轉的矩陣
// v 須先正規化
// fSin 和 fCos 須是同一個旋轉角度的三角函數值
inline Matrix4x4 cxxlFASTCALL Rotate(const Vector4 &v, const C3D_FLOAT fSin, const C3D_FLOAT fCos)
{
  Matrix4x4 tmp;

  tmp.m_00 = ( v.x * v.x ) * ( 1.0f - fCos ) + fCos;
  tmp.m_01 = ( v.x * v.y ) * ( 1.0f - fCos ) + (v.z * fSin);
  tmp.m_02 = ( v.x * v.z ) * ( 1.0f - fCos ) - (v.y * fSin);
  tmp.m_03 = 0.0f;

  tmp.m_10 = ( v.y * v.x ) * ( 1.0f - fCos ) - (v.z * fSin);
  tmp.m_11 = ( v.y * v.y ) * ( 1.0f - fCos ) + fCos ;
  tmp.m_12 = ( v.y * v.z ) * ( 1.0f - fCos ) + (v.x * fSin);
  tmp.m_13 = 0.0f;

  tmp.m_20 = ( v.z * v.x ) * ( 1.0f - fCos ) + (v.y * fSin);
  tmp.m_21 = ( v.z * v.y ) * ( 1.0f - fCos ) - (v.x * fSin);
  tmp.m_22 = ( v.z * v.z ) * ( 1.0f - fCos ) + fCos;
  tmp.m_23 = 0.0f;

  tmp.m_30 = 0.0f;
  tmp.m_31 = 0.0f;
  tmp.m_32 = 0.0f;
  tmp.m_33 = 1.0f;

  return tmp;
}


// 產生圍繞指定向量旋轉的矩陣
// v 須先正規化
inline Matrix4x4 cxxlFASTCALL Rotate(const Vector4 &v, const C3D_FLOAT radian)
{
	C3D_FLOAT fSin, fCos;
	SinCos(radian, fSin, fCos);

  return Rotate(v, fSin, fCos);
}

// 產生位繞指定向量縮放的矩陣
inline Matrix4x4 cxxlFASTCALL Scale(C3D_FLOAT x, C3D_FLOAT y, C3D_FLOAT z)
{
  return Matrix4x4(
    x, 0, 0, 0,
    0, y, 0, 0,
    0, 0, y, 0,
    0, 0, 0, 1
  );
}

// 向量 * 矩陣
inline C3DVector4 cxxlFASTCALL operator*(const C3DVector4 &v, const Matrix4x4 &m)
{
//  CxxlMan::CriticalSecHandle AutoCSH(m.csObject);

  return C3DVector4(
    v.x*m.m_00 + v.y*m.m_10 + v.z*m.m_20 + v.w*m.m_30,
    v.x*m.m_01 + v.y*m.m_11 + v.z*m.m_21 + v.w*m.m_31,
    v.x*m.m_02 + v.y*m.m_12 + v.z*m.m_22 + v.w*m.m_32,
    v.x*m.m_03 + v.y*m.m_13 + v.z*m.m_23 + v.w*m.m_33
  );

}

// 向量 * 矩陣
inline Vector4 cxxlFASTCALL operator*(const Vector4 &v, const Matrix4x4 &m)
{
  return v.Get_C3DVector4() * m;
}


// 矩陣 * 矩陣
inline Matrix4x4 cxxlFASTCALL operator*(const Matrix4x4 &m1, const Matrix4x4 &m2)
{
//  Matrix4x4 tmp(m2); // 在 Copy Constructor 有同步控制

//  CxxlMan::CriticalSecHandle AutoCSH(m1.csObject);
  return Matrix4x4(
    m1.m_Vec0 * m2,
    m1.m_Vec1 * m2,
    m1.m_Vec2 * m2,
    m1.m_Vec3 * m2
  );
}




// 座標轉換矩陣的反矩陣函數
inline Matrix4x4 cxxlFASTCALL CoordinateInvert(const Matrix4x4 &m)
{
  static const Matrix4x4 mt(
    1, 0, 0, 1, // xaxis
    0, 1, 0, 1, // yaxis
    0, 0, 1, 1, // zaxis
    0, 0, 0, 1  // 原點
  );

  Matrix4x4 axis = mt * m;
  axis.m_Vec0 = axis.m_Vec0 - axis.m_Vec3;
  axis.m_Vec1 = axis.m_Vec1 - axis.m_Vec3;
  axis.m_Vec2 = axis.m_Vec2 - axis.m_Vec3;

  // 處理縮放 Vector3Normalize

  C3D_FLOAT Vec0Length = Vector3Length(axis.m_Vec0); Vector3Normalize(axis.m_Vec0); axis.m_Vec0 = axis.m_Vec0 / Vec0Length;
  C3D_FLOAT Vec1Length = Vector3Length(axis.m_Vec1); Vector3Normalize(axis.m_Vec1); axis.m_Vec1 = axis.m_Vec1 / Vec1Length;
  C3D_FLOAT Vec2Length = Vector3Length(axis.m_Vec2); Vector3Normalize(axis.m_Vec2); axis.m_Vec2 = axis.m_Vec2 / Vec2Length;
  // C3D_FLOAT Vec3Length = Vector3Length(axis.m_Vec3); Vector3Normalize(axis.m_Vec3); axis.m_Vec3 = axis.m_Vec3 / Vec3Length;

  Matrix4x4 r = Matrix4x4::IdentityCreate();

 	r.SetColumn(0, axis.GetRow(0));
	r.SetColumn(1, axis.GetRow(1));
	r.SetColumn(2, axis.GetRow(2));

  // 位移
  r[3][0] = -Vector3Dot(axis.m_Vec0, axis.m_Vec3);
	r[3][1] = -Vector3Dot(axis.m_Vec1, axis.m_Vec3);
	r[3][2] = -Vector3Dot(axis.m_Vec2, axis.m_Vec3);

  return r;
}

/*
// 座標轉換矩陣的反矩陣函數
inline Matrix4x4 cxxlFASTCALL CoordinateInvert(const Matrix4x4 &m)
{
  const Matrix4x4 mt(
    1, 0, 0, 1, // xaxis
    0, 1, 0, 1, // yaxis
    0, 0, 1, 1, // zaxis
    0, 0, 0, 1  // 原點
  );

  Matrix4x4 axis = mt * m;
  axis.m_Vec0 = axis.m_Vec0 - axis.m_Vec3;
  axis.m_Vec1 = axis.m_Vec1 - axis.m_Vec3;
  axis.m_Vec2 = axis.m_Vec2 - axis.m_Vec3;

  // 縮放
  axis.m_Vec0 = axis.m_Vec0 * (1.0/VectorLength(axis.m_Vec0));
  axis.m_Vec1 = axis.m_Vec1 * (1.0/VectorLength(axis.m_Vec1));
  axis.m_Vec2 = axis.m_Vec2 * (1.0/VectorLength(axis.m_Vec2));

  Matrix4x4 r;
  r.Identity();

 	r.SetColumn(0, axis.GetRow(0));
	r.SetColumn(1, axis.GetRow(1));
	r.SetColumn(2, axis.GetRow(2));

  // 位移
  r[3][0] = -VectorDot(axis.m_Vec0, axis.m_Vec3);
	r[3][1] = -VectorDot(axis.m_Vec1, axis.m_Vec3);
	r[3][2] = -VectorDot(axis.m_Vec2, axis.m_Vec3);

  return r;
}
*/

}   /* namespace Cxxl3D */
#endif

